/*
 * cDOPRI.cpp
 *
 *  Created on: Nov 8, 2010
 *      Author: murmeli
 */

#include "cDOPRI.h"
#include "../util/convert.h"

DOPRI::DOPRI() {
	setName("Dormand - Prince");
	setMinerror(0.0);
	setMaxerror(5e-3);
	setMinstep(1e-12);
	setMaxstep(5.0);
}

void DOPRI::methodStep(double & step, vector<double>& xx, vector<double>& yy, vector<double>& dy, cDY* DY, cPot* V) {
	double k1,k2,k3,k4,k5,k6,k7,l1,l2,l3,l4,l5,l6,l7,E_est,E_est4,E_est5;
		bool done=false;
		while(done==false) {
	//Calculate single step
			k1 = dy.back();
			l1 = DY->DE(xx.back(), V->potential(xx.back()), yy.back(), dy.back());

			k2 = dy.back() + (1.0/5.0)*step * l1;
			l2 = DY->DE(xx.back() + (1.0/5.0)*step, V->potential(xx.back() + (1.0/5.0)*step), yy.back() + (1.0/5.0)*step * k1, k2);

			k3 = dy.back() + step*((3.0/40.0) * l1 + (9.0/40.0)*l2);
			l3 = DY->DE(xx.back() + (3.0/10.0)*step, V->potential(xx.back() + (3.0/10.0)*step), yy.back() + step * ((3.0/40.0) * k1 + (9.0/40.0) * k2),k3);

			k4 = dy.back() + step*((44.0/45.0)*l1 - (56.0/15.0)*l2 + (32.0/9.0)*l3);
			l4 = DY->DE(xx.back() + (4.0/5.0)*step, V->potential(xx.back() + (4.0/5.0)*step), yy.back() + step*((44.0/45.0)*k1 -(56.0/15.0)*k2 + (32.0/9.0)*k3), k4);

			k5 = dy.back() + step*((19372.0/6561.0)*l1 - (25360.0/2187.0)*l2 + (64448.0/6561.0)*l3 - (212.0/729.0)* l4);
			l5 = DY->DE(xx.back()+(8.0/9.0)*step, V->potential(xx.back() + (8.0/9.0)*step), yy.back() + step*((19372.0/6561.0)*k1 - (25360.0/2187.0)*k2 + (64448.0/6561.0)*k3 - (212.0/729.0)* k4), k5);

			k6 = dy.back() + step*( (9017.0/3168.0)*l1 - (355.0/33.0)*l2 + (46732.0/5247.0)*l3 + (49.0/176.0)*l4 - (5103.0/18656.0)*l5);
			l6 = DY->DE(xx.back() + step, V->potential(xx.back() + step), yy.back() + step*( (9017.0/3168.0)*k1 - (355.0/33.0)*k2 + (46732.0/5247.0)*k3 + (49.0/176.0)*k4 - (5103.0/18656.0)*k5), k6);

			k7 = dy.back() + step*( (35.0/384.0)*l1 + (500.0/1113.0)*l3 + (125.0/192.0)*l4 - (2187.0/6784.0)*l5 + (11.0/84.0)*l6);
			l7 = DY->DE(xx.back() + step, V->potential(xx.back() + step), yy.back() + step*( (35.0/384.0)*k1 + (500.0/1113.0)*k3 + (125.0/192.0)*k4 - (2187.0/6784.0)*k5 + (11.0/84.0)*k6), k7);

	//Calculate the error estimate for the step
			E_est4 = step*((5179.0/57600.0)*k1 + (7571.0/16695.0)*k3 + (393.0/640.0)*k4 - (92097.0/339200.0)*k5 + (187.0/2100.0)*k6 + (1.0/40.0)*k7);
			E_est5 = step*((35.0/384.0)*k1 + (500.0/1113.0)*k3 + (125.0/192.0)*k4 - (2187.0/6784.0)*k5 + (11.0/84.0)*k6);
			E_est = fabs(E_est5 - E_est4);
		//	cout << "Error = " << E_est <<  " step = " << step << endl;

	//If the error estimate is less than specified maximum error and larger than specified minimum error we are done.
			if(E_est < getMaxerror() && E_est > getMinerror()) {
				done = true;
			}
	//If the error estimate is smaller than specified minimum error we double the step size and calculate the step again (done = false).
			else if(E_est <= getMinerror()) {
				step = 2.0*step;
				done = false;
			}
	//If the error estimate is larger than the specified maximum error we calculate a new step size and try to calculate the step again.
			else if(E_est >= getMaxerror()) {
				step = 0.9*step*pow(getMaxerror()/E_est,(1.0/5.0));
				//step = step*0.5;
			//	cout << "step size = " << step << endl;
				//If the new step size is smaller than the specified minimum step size we throw an exception and calculation stops.
				if(fabs(step) < getMinstep()) {
                                    string s1 = "Minimum step size reached at x = " + tostring(xx.back());
                                    throw(cNumericalError(s1.c_str()));
                                }
				done = false;
			}
		}

		yy.push_back(yy.back() + E_est5);
		dy.push_back(dy.back() + step * ((35.0/384.0)*l1 + (500.0/1113.0)*l3 + (125.0/192.0)*l4 - (2187.0/6784.0)*l5 + (11.0/84.0)*l6));
		xx.push_back(xx.back() + step);
}
